#include <general_interface/msg/control_signal.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <nav_msgs/msg/path.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <utility>
#include <vector>
#include <visualization_msgs/msg/marker.hpp>

/*
 *  FollowPrediction for path tracking
 */

class FollowPrediction : public rclcpp::Node
{
  private:
    rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr pathSub;
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odomSub;
    rclcpp::Publisher<general_interface::msg::ControlSignal>::SharedPtr
        controlPub;

    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr markerPub;

    std::vector<tf2::Vector3> path{tf2::Vector3(0, 0, 0)};
    size_t index = 0;
    double xscale = 1;
    double yscale = 1;
    double prediction_distance = 1;

  public:
    FollowPrediction() : Node("follow_prediction")
    {
        RCLCPP_INFO(this->get_logger(), "follow_prediction node started");

        declare_parameter("xscale", 1.0);
        declare_parameter("yscale", 1.0);
        get_parameter("xscale", xscale);
        get_parameter("yscale", yscale);
        declare_parameter("prediction_distance", 2.0);
        get_parameter("prediction_distance", prediction_distance);

        pathSub = this->create_subscription<nav_msgs::msg::Path>(
            "reference_path", 10,
            [this](const nav_msgs::msg::Path::SharedPtr msg)
            {
                path.clear();
                path.resize(msg->poses.size());
                for (size_t i = 0; i < msg->poses.size(); i++)
                {
                    tf2::fromMsg(msg->poses[i].pose.position, path[i]);
                }
            });
        odomSub = this->create_subscription<nav_msgs::msg::Odometry>(
            "odom", 10,
            [this](const nav_msgs::msg::Odometry::SharedPtr msg)
            {
                tf2::Vector3 position;
                tf2::Quaternion orientation;
                tf2::fromMsg(msg->pose.pose.position, position);
                tf2::fromMsg(msg->pose.pose.orientation, orientation);
                auto [linear, angular] = CreatSolution(position, orientation);
                general_interface::msg::ControlSignal controlSignal;
                controlSignal.forward_control = linear;
                controlSignal.rotate_control = angular;
                controlPub->publish(controlSignal);
            });
        controlPub =
            this->create_publisher<general_interface::msg::ControlSignal>(
                "control_signal", 10);
        markerPub = this->create_publisher<visualization_msgs::msg::Marker>(
            "target", 10);
    }

    std::pair<double, double> CreatSolution(const tf2::Vector3 &position,
                                            const tf2::Quaternion &orientation)
    {
        if (path.size() < 2)
            return std::make_pair(0, 0);
        while (path[index].distance(position) < prediction_distance)
        {
            index++;
            if (index >= path.size() - 1)
            {
                index = 0;
                RCLCPP_INFO(get_logger(), "arrive end, finish path follow");
                return std::make_pair(0, 0);
            }
        }
        tf2::Vector3 target = path.at(index);
        // RCLCPP_INFO(get_logger(), "target:%f,%f,%f", target.x(), target.y(),
        //             target.z());
        MarkerPublish(target);
        // double xe = target.distance(position);
        tf2::Vector3 forward =
            tf2::quatRotate(orientation, tf2::Vector3(1, 0, 0));
        double ae = tf2::tf2Angle(target - position, forward);
        double sign = (target - position).cross(forward).z() > 0 ? -1 : 1;
        // RCLCPP_INFO(get_logger(), "xe: %f, ae: %f", xe, ae * sign);
        return std::make_pair(std::numbers::pi / 180 * 1, ae * yscale * sign);
    }

    static std::vector<tf2::Vector3> SamplePoint(
        const std::vector<tf2::Vector3> &path, const tf2::Vector3 &position,
        double d)
    {
        std::vector<tf2::Vector3> results;
        double lastDistance = path[0].distance(position);
        double lastSign = lastDistance - d;
        for (size_t i = 1; i < path.size(); i++)
        {
            double distance = path[i].distance(position);
            double sign = distance - d;
            if (sign * lastSign < 0)
            {
                // possible point, need check
                double precent = (d - lastDistance) / (distance - lastDistance);
                tf2::Vector3 dp =
                    path[i - 1] + precent * (path[i] - path[i - 1]);
                results.push_back(dp);
            }
            lastSign = sign;
            lastDistance = distance;
        }
        return results;
    }

  private:
    void MarkerPublish(const tf2::Vector3 &position)
    {
        visualization_msgs::msg::Marker marker;
        marker.header.frame_id = "map";
        marker.header.stamp = this->get_clock()->now();
        marker.ns = "follow_prediction";
        marker.id = 0;
        marker.type = visualization_msgs::msg::Marker::SPHERE;
        marker.action = visualization_msgs::msg::Marker::ADD;
        tf2::toMsg(position, marker.pose.position);
        marker.pose.orientation = tf2::toMsg(tf2::Quaternion(0, 0, 0, 1));
        marker.scale.x = 0.5;
        marker.scale.y = 0.5;
        marker.scale.z = 0.5;
        marker.color.a = 1.0;
        marker.color.r = 1.0;
        marker.color.g = 0.0;
        marker.color.b = 0.0;
        marker.lifetime = rclcpp::Duration::from_seconds(1);
        markerPub->publish(marker);
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<FollowPrediction>());
    rclcpp::shutdown();
    return 0;
}
